/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * File Name          : ekf.hpp
  * Description        : 
  ******************************************************************************
  * Function           :
  *
  ******************************************************************************
  * Author             : Zhiping Fu
  * Creation Date      : 2020年11月12日
  ******************************************************************************
  */
/* USER CODE END Header */

#ifndef __EKF_HPP
#define __EKF_HPP

#include "userlib/userlib.hpp"
#include "system/system.hpp"


#ifdef __cplusplus

#include <Eigen>

using namespace std;
using namespace Eigen;

class EKF
{
public:
	EKF(){}
	EKF(char *n) : name(n){}
	~EKF(){}
	void Ekf_Init();
	void Ekf_Update();

private:
	char *name;
	sTIM Tim;
	bool Update;
	uint32_t startTimer;
	uint32_t stopTimer;
	uint32_t  executionTime_us;
	float   AccUpdate_time;//更新时间
	float   AccDeltaT_Sample;
	float   GyroUpdate_time;//更新时间
	float   GyroDeltaT_Sample;
	float   MagUpdate_time;//更新时间
	float   MagDeltaT_Sample;


	//矩阵运算变量定义（EKF）
	Matrix<float,4,6> H;		//灵敏度矩阵
	Matrix<float,6,4> K;		//kalman gain
	Matrix<float,6,6> P_p;		//协方差矩阵预测
	Matrix<float,6,6> P_k;		//协方差矩阵更新
	Matrix<float,4,4> R;		//测量噪声
	Matrix<float,6,6> Q;		//过程噪声
	Matrix<float,4,1> y;					//测量值
	Matrix<float,4,1> y_p;				//预测测量
	Matrix<float,7,1> X_p;		//状态预测
	Matrix<float,7,1> X_k;		//状态更新
	Matrix<float,6,1> X_k_error;		//状态误差更新
	Matrix<float,6,6> F;
	Matrix<float,6,6> G;

	Vector3f MatAcc; //加速度计数据
	Vector3f MatGyr; //陀螺仪数据
	Vector3f MatMag; //磁力计数据

	Quaternion<float> q_p;  //四元数
	Quaternion<float> q_k;  //四元数更新值
	Matrix<float,3,3> Cn2b;	//NED到机体坐标系的旋转矩阵
	Matrix<float,4,3> Mq;

	Vector3f Euler;  //欧拉角
	Vector3f a_k;
	Vector3f w_k;
	float dt;
	Vector3f g;
	Vector3f gb;
	Vector3f M;
	Vector3f Mb;



	sensor_gyro_msg gyro;
	sensor_acc_msg acc;
	sensor_mag_msg mag;
	ekf_msg ekfAngUvw;
	flow_msg flow;
	height_msg height;

};

#endif




#endif


/************************ (C) COPYRIGHT Longmen Drone Team *****END OF FILE****/
